<pre class="code">
<span class="srcline"><span class="lineno"><a href="46,1" id="srcline1"> 1</a></span><span class="line"><span class="comment">%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,2" id="srcline2"> 2</a></span><span class="line"><span class="comment">% RPI-MATLAB-Simulator</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,3" id="srcline3"> 3</a></span><span class="line"><span class="comment">% http://code.google.com/p/rpi-matlab-simulator/</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,4" id="srcline4"> 4</a></span><span class="line"><span class="comment">%</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,5" id="srcline5"> 5</a></span><span class="line"><span class="comment">% Adds the column vector of body structs to the sim struct.  </span></span></span>
<span class="srcline"><span class="lineno"><a href="46,6" id="srcline6"> 6</a></span><span class="line"><span class="comment">% INPUTS:</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,7" id="srcline7"> 7</a></span><span class="line"><span class="comment">%       sim  - A simulation struct created with Simulator().</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,8" id="srcline8"> 8</a></span><span class="line"><span class="comment">%       body - A body struct or vector of body </span></span></span>
<span class="srcline"><span class="lineno"><a href="46,9" id="srcline9"> 9</a></span><span class="line"><span class="comment">%               structs created with Body().  </span></span></span>
<span class="srcline"><span class="lineno"><a href="46,10" id="srcline10">10</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="46,11" id="srcline11">11</a></span><span class="line"><span class="keyword">function</span> <span class="var type1" id="S2T6U3">sim</span> = sim_addBody( <span class="mxinfo" id="T6:U2"><span class="mxinfo" id="T6:U3"><span class="var type1" id="S2T6U6">sim</span></span></span>, <span class="var type1" id="S3T36U7">body</span> )</span></span>
<span class="srcline"><span class="lineno"><a href="46,12" id="srcline12">12</a></span><span class="line"><span class="comment">%#codegen</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,13" id="srcline13">13</a></span><span class="line">    <span class="comment">% Allow body to contain multiple bodies, but require a vector.</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,14" id="srcline14">14</a></span><span class="line">    <span class="mxinfo" id="T1:U6">[<span class="var type1" id="S4T1U11">r</span>, <span class="var type1" id="S5T1U12">c</span>] = size(<span class="var type1" id="S3T27U15">body</span>)</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="46,15" id="srcline15">15</a></span><span class="line">    <span class="keyword">if</span> min(<span class="var type1" id="S4T1U21">r</span>,<span class="var type1" id="S5T1U22">c</span>) &gt; 1 </span></span>
<span class="srcline"><span class="lineno"><a href="46,16" id="srcline16">16</a></span><span class="line">       error(<span class="string">'Attempted to add a matrix of bodies to the simulator.'</span>); </span></span>
<span class="srcline"><span class="lineno"><a href="46,17" id="srcline17">17</a></span><span class="line">    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,18" id="srcline18">18</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="46,19" id="srcline19">19</a></span><span class="line">    <span class="comment">% Force a row vector.</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,20" id="srcline20">20</a></span><span class="line">    <span class="keyword">if</span> <span class="var type1" id="S4T1U31">r</span> &gt; 1</span></span>
<span class="srcline"><span class="lineno"><a href="46,21" id="srcline21">21</a></span><span class="line">        <span class="var type2" id="S3T0U35">body</span> = <span class="var type2" id="S3T0U37">body</span>'; </span></span>
<span class="srcline"><span class="lineno"><a href="46,22" id="srcline22">22</a></span><span class="line">    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,23" id="srcline23">23</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="46,24" id="srcline24">24</a></span><span class="line">    <span class="comment">% Update body information</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,25" id="srcline25">25</a></span><span class="line">    <span class="keyword">for</span> <span class="var type1" id="S9T1U40">b</span>=<span class="mxinfo" id="T1:U14">1</span>:length(<span class="var type1" id="S3T27U45">body</span>)</span></span>
<span class="srcline"><span class="lineno"><a href="46,26" id="srcline26">26</a></span><span class="line">       <span class="comment">% Force position vectors to be column vectors</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,27" id="srcline27">27</a></span><span class="line">       <span class="keyword">if</span> size(<span class="var type1" id="S3T27U53">body</span>(<span class="var type1" id="S9T1U54">b</span>).u,2) &gt; 1, <span class="var type2" id="S3T0U62">body</span>(<span class="var type2" id="S9T0U63">b</span>).u = <span class="var type2" id="S3T0U68">body</span>(<span class="var type2" id="S9T0U69">b</span>).u'; <span class="keyword">end</span> </span></span>
<span class="srcline"><span class="lineno"><a href="46,28" id="srcline28">28</a></span><span class="line">       <span class="mxinfo" id="T34:U18"><span class="mxinfo" id="T34:U19"><span class="var type1" id="S3T36U74">body</span>(<span class="var type1" id="S9T1U75">b</span>)</span> = <span class="mxinfo" id="T34:U22"><span class="fcn" id="F91N8:B77">body_updatePosition</span>(<span class="mxinfo" id="T34:U23"><span class="var type1" id="S3T36U79">body</span>(<span class="var type1" id="S9T1U80">b</span>)</span>,<span class="mxinfo" id="T1:U26">0</span>)</span></span>;  </span></span>
<span class="srcline"><span class="lineno"><a href="46,29" id="srcline29">29</a></span><span class="line">       <span class="keyword">if</span> <span class="mxinfo" id="T4:U27">strcmp(<span class="mxinfo" id="T33:U28"><span class="mxinfo" id="T34:U29"><span class="var type1" id="S3T36U88">body</span>(<span class="var type1" id="S9T1U89">b</span>)</span>.type</span>,<span class="string">'mesh'</span>)</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,30" id="srcline30">30</a></span><span class="line">          <span class="mxinfo" id="T34:U32"><span class="mxinfo" id="T34:U33"><span class="var type1" id="S3T36U95">body</span>(<span class="var type1" id="S9T1U96">b</span>)</span> = <span class="mxinfo" id="T34:U36"><span class="fcn" id="F93N7:B98">body_updateMesh</span>(<span class="mxinfo" id="T34:U37"><span class="var type1" id="S3T36U100">body</span>(<span class="var type1" id="S9T1U101">b</span>)</span>)</span></span>;  </span></span>
<span class="srcline"><span class="lineno"><a href="46,31" id="srcline31">31</a></span><span class="line">       <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,32" id="srcline32">32</a></span><span class="line">       <span class="mxinfo" id="T1:U40"><span class="mxinfo" id="T1:U41"><span class="mxinfo" id="T34:U42"><span class="var type1" id="S3T36U106">body</span>(<span class="var type1" id="S9T1U107">b</span>)</span>.bodyID</span> = <span class="mxinfo" id="T1:U45"><span class="mxinfo" id="T1:U46"><span class="var type1" id="S2T6U111">sim</span>.num_bodies</span> + <span class="var type1" id="S9T1U113">b</span></span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="46,33" id="srcline33">33</a></span><span class="line">    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,34" id="srcline34">34</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="46,35" id="srcline35">35</a></span><span class="line">    <span class="comment">% Update and return the sim struct. </span></span></span>
<span class="srcline"><span class="lineno"><a href="46,36" id="srcline36">36</a></span><span class="line">    <span class="comment">%sim.bodies = [ sim.bodies body ];</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,37" id="srcline37">37</a></span><span class="line">    <span class="mxinfo" id="T1:U49"><span class="mxinfo" id="T1:U50"><span class="var type1" id="S2T6U117">sim</span>.num_bodies</span> = <span class="mxinfo" id="T1:U52"><span class="mxinfo" id="T1:U53"><span class="var type1" id="S2T6U121">sim</span>.num_bodies</span> + <span class="mxinfo" id="T1:U55">length(<span class="var type1" id="S3T27U125">body</span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="46,38" id="srcline38">38</a></span><span class="line">    <span class="keyword">for</span> <span class="var type1" id="S14T1U128">i</span>=<span class="mxinfo" id="T1:U58">1</span>:length(<span class="var type1" id="S3T27U133">body</span>)</span></span>
<span class="srcline"><span class="lineno"><a href="46,39" id="srcline39">39</a></span><span class="line">      <span class="keyword">if</span> (<span class="mxinfo" id="T4:U60"><span class="mxinfo" id="T34:U61"><span class="var type1" id="S3T36U139">body</span>(<span class="var type1" id="S14T1U140">i</span>)</span>.dynamic</span>)</span></span>
<span class="srcline"><span class="lineno"><a href="46,40" id="srcline40">40</a></span><span class="line">        <span class="mxinfo" id="T1:U64"><span class="mxinfo" id="T1:U65"><span class="var type1" id="S2T6U145">sim</span>.num_dynamicBodies</span> = <span class="mxinfo" id="T1:U67"><span class="mxinfo" id="T1:U68"><span class="var type1" id="S2T6U149">sim</span>.num_dynamicBodies</span> + <span class="mxinfo" id="T1:U70">1</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="46,41" id="srcline41">41</a></span><span class="line">        <span class="comment">%sim.bodies(i).bodyDynamicID = sim.num_dynamicBodies;</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,42" id="srcline42">42</a></span><span class="line">      <span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,43" id="srcline43">43</a></span><span class="line">        <span class="comment">%sim.bodies(i).bodyDynamicID = -1;</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,44" id="srcline44">44</a></span><span class="line">      <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,45" id="srcline45">45</a></span><span class="line">    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,46" id="srcline46">46</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="46,47" id="srcline47">47</a></span><span class="line"></span></span>
</pre>
